#include "FPmath.h"

Fixed FPmath::abs (const Fixed& f)
{
    if (f < 0) return -f; return f;
}

Fixed FPmath::floor (const Fixed& f)
{
    return (f & (-1 << DECIMAL_BITS));
}

Fixed FPmath::ceil (const Fixed& f)
{
    return (f & (ONE -1)) ? (f & (-1 << DECIMAL_BITS)) + 1 : f;
}

Fixed FPmath::min (const Fixed& a, const Fixed& b)
{
    return (a<b) ? a : b;
}

Fixed FPmath::max (const Fixed& a, const Fixed& b)
{
     return (a>b) ? a : b;
}

Fixed FPmath::sqrt(const Fixed& f)
{
    register Fixed s,r;

    if (f < ONE) { return 0; }

    s = f >> 2;

    do
    {
        r=s;
        s = ( s + f/s) >> 1;
    } while (r != s );

    return r;
}

//A fast but less acurate method
//This method is very cheap and runs near the spped of a divide operation!
//Use where possible
Fixed FPmath::sqrt2(const Fixed& f)
{
    register int op=f.raw;
    register int res=0;

    if (f < ONE) { return 0; }

    /* "one" starts at the highest power of four <= than the argument. */
    int one = 1 << 30;  /* second-to-top bit set */
    while (one > op) one >>= 2;

    while (one != 0) {
        if (op >= res + one) {
            op = op - (res + one);
            res = res +  (one << 1);  // <-- faster than 2 * one
        }
        res >>= 1;
        one >>= 2;
    }

    res <<= 8;
    return Fixed(res,0);
}


// Return an approx to sin(rad)
// It has a max absolute error of 5e-9
// according to Hastings, Approximations For Digital Computers.
// Since we are using fixed-point the accuracy is more around 5e-6
Fixed FPmath::sin(const Fixed& rad)
{
    Fixed x=(rad * 2 ) / FPmath::PI;

    //These improve accuracy
    while (x>1) {x-=2;}
    while (x<-1) {x+=2;}

    Fixed x2 = x * x;
    return ((((x2 * Fixed(10,0) - Fixed(306,0)) * x2 + Fixed(5222,0)) * x2
        - Fixed(42333,0)) * x2 + Fixed(102942,0)) * x;
}


Fixed FPmath::cos(const Fixed& rad) {
    return FPmath::sin (rad+ (PI>>1));
}

Fixed FPmath::tan( const Fixed& rad) {
    //TODO: there is probibly a faster way to compute this
    Fixed t=cos(rad);
    if (t==0)
        return t; //TODO: I really should return something instead of 0 for NaN
    return sin(rad)/t;
}
